#include <iostream>
#include <Eigen/Eigen>
#include <ros/ros.h>
#include "bspline_cius/non_uniform_bspline_cius.h"



using namespace std;
using namespace Eigen;

int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "test_bspline");
    ros::NodeHandle nh("~");

	Eigen::MatrixXd control_points(7,3);


	control_points<<
	0,0,0,
	0.5,0.5,0,
	1,1,0,
	1.5,0.5,0,
	2,0,0,
	3,-1,0,
	4,0,0;


	trajectory_formation::NonUniformBspline bspline(control_points,3,0.25);
	std::cout<<"bspline.u:"<<bspline.u_<<std::endl;

	bspline.resetKnot(1,4,1);

	std::cout<<"after reset bspline.u:\n"<<bspline.u_<<std::endl;
	std::cout<<"after reset bspline.duration:\n"<<bspline.duration_time<<std::endl;

	for(int i=0;;i++)
	{
		double t_cur=i*0.01;
		if(t_cur>bspline.duration_time)break;
		std::vector<double> total_coff;
		int start_idx;
		Eigen::Vector3d pos=bspline.evaluateDeBoorTandCoefficients(t_cur,total_coff,start_idx);

		// ROS_INFO("total_coff: %f %f %f %f %f %f %f point: %f %f %f",total_coff[0],total_coff[1],total_coff[2],total_coff[3],total_coff[4],total_coff[5],
		// total_coff[6],pos(0),pos(1),pos(2));

		// std::cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<","<<std::endl;

		// ROS_INFO("time: %f  pos: %f %f %f", t_cur,pos(0),pos(1),pos(2)   );
	}

	// ROS_INFO("M: %d time_sum: %f",	bspline.m_,bspline.getTimeSum());
	// std::cout<<"control points:\n"<<bspline.control_points_<<std::endl;

	// std::cout<<"de control points: \n"<<bspline.getDerivativeControlPoints()<<std::endl;

	// trajectory_formation::NonUniformBspline de_bspline=bspline.getDerivative();

	// for(int i=0;i<de_bspline.u_.size();i++)
	// {
	// 	ROS_INFO("de spline knot: %f",de_bspline.u_[i]);
	// }

	// for(int i=0;i<300;i++)
	// {
	// 	double t_cur=i*0.01;
	// 	if(t_cur>bspline.duration_time)break;
	// 	std::vector<double> total_coff;
	// 	int start_idx;
	// 	Eigen::Vector3d pos=bspline.evaluateDeBoorTandCoefficients(t_cur,total_coff,start_idx);

	// 	ROS_INFO("total_coff: %f %f %f %f %f %f %f point: %f %f %f",total_coff[0],total_coff[1],total_coff[2],total_coff[3],total_coff[4],total_coff[5],
	// 	total_coff[6],pos(0),pos(1),pos(2));

	// 	// ROS_INFO("time: %f  pos: %f %f %f", t_cur,pos(0),pos(1),pos(2)   );
	// }








	ROS_INFO("//////////////////////////////");

	// std::vector<Eigen::Vector3d> waypoint;
	// Eigen::MatrixXd waypoint_matrix(5,3);
	// waypoint_matrix<<
	// 				0,0,0,
	// 				0.5,0.5,0,
	// 				1,1,0,
	// 				1.5,0.5,0,
	// 				2,0,0;

	// for(int i=0;i<waypoint_matrix.rows();i++)
	// {
	// 	waypoint.push_back(waypoint_matrix.row(i));
	// }
	// std::vector<Eigen::Vector3d> point_set, start_end_derivatives;
	// Eigen::Vector3d tmp_v(0.5,1,0);
	// start_end_derivatives.push_back(tmp_v);
	// start_end_derivatives.push_back(Eigen::Vector3d::Zero());
	// start_end_derivatives.push_back(Eigen::Vector3d::Zero());
	// start_end_derivatives.push_back(Eigen::Vector3d::Zero());
	// Eigen::MatrixXd ctrl_pts;
	// double ts=0.25;
	// trajectory_formation::parameterizeToBspline(ts, waypoint, start_end_derivatives, ctrl_pts);

	//  bspline.setUniformBspline(ctrl_pts,3,ts);

	//  std::cout<<"ctrl_pts: \n"<<ctrl_pts<<std::endl;

	// for(int i=0;;i++)
	// {
	// 	double t_cur=i*0.01;
	// 	if(t_cur>bspline.duration_time)break;
	// 	Eigen::Vector3d pos=bspline.evaluateDeBoorT(t_cur);
	// 	ROS_INFO("time: %f  pos: %f %f %f", t_cur,pos(0),pos(1),pos(2)   );
	// }



}